/**
 * @file math_utils.c
 * @brief Math utilities implementation
 */

#include "math_utils.h"

float constrain_float(float val, float min, float max)
{
    if (val < min) return min;
    if (val > max) return max;
    return val;
}

float wrap_pi(float angle)
{
    while (angle > PI) angle -= TWO_PI;
    while (angle < -PI) angle += TWO_PI;
    return angle;
}

float wrap_2pi(float angle)
{
    while (angle >= TWO_PI) angle -= TWO_PI;
    while (angle < 0.0f) angle += TWO_PI;
    return angle;
}

void vector3_add(const Vector3_t *a, const Vector3_t *b, Vector3_t *result)
{
    result->x = a->x + b->x;
    result->y = a->y + b->y;
    result->z = a->z + b->z;
}

void vector3_sub(const Vector3_t *a, const Vector3_t *b, Vector3_t *result)
{
    result->x = a->x - b->x;
    result->y = a->y - b->y;
    result->z = a->z - b->z;
}

float vector3_dot(const Vector3_t *a, const Vector3_t *b)
{
    return a->x * b->x + a->y * b->y + a->z * b->z;
}

void vector3_cross(const Vector3_t *a, const Vector3_t *b, Vector3_t *result)
{
    result->x = a->y * b->z - a->z * b->y;
    result->y = a->z * b->x - a->x * b->z;
    result->z = a->x * b->y - a->y * b->x;
}

float vector3_length(const Vector3_t *v)
{
    return sqrtf(v->x * v->x + v->y * v->y + v->z * v->z);
}

void vector3_normalize(Vector3_t *v)
{
    float len = vector3_length(v);
    if (len > 0.0f) {
        v->x /= len;
        v->y /= len;
        v->z /= len;
    }
}

void quaternion_from_euler(float roll, float pitch, float yaw, Quaternion_t *q)
{
    float cr = cosf(roll * 0.5f);
    float sr = sinf(roll * 0.5f);
    float cp = cosf(pitch * 0.5f);
    float sp = sinf(pitch * 0.5f);
    float cy = cosf(yaw * 0.5f);
    float sy = sinf(yaw * 0.5f);
    
    q->w = cr * cp * cy + sr * sp * sy;
    q->x = sr * cp * cy - cr * sp * sy;
    q->y = cr * sp * cy + sr * cp * sy;
    q->z = cr * cp * sy - sr * sp * cy;
}

void quaternion_to_euler(const Quaternion_t *q, float *roll, float *pitch, float *yaw)
{
    /* Roll (x-axis rotation) */
    float sinr_cosp = 2.0f * (q->w * q->x + q->y * q->z);
    float cosr_cosp = 1.0f - 2.0f * (q->x * q->x + q->y * q->y);
    *roll = atan2f(sinr_cosp, cosr_cosp);
    
    /* Pitch (y-axis rotation) */
    float sinp = 2.0f * (q->w * q->y - q->z * q->x);
    if (fabsf(sinp) >= 1.0f) {
        *pitch = copysignf(HALF_PI, sinp);
    } else {
        *pitch = asinf(sinp);
    }
    
    /* Yaw (z-axis rotation) */
    float siny_cosp = 2.0f * (q->w * q->z + q->x * q->y);
    float cosy_cosp = 1.0f - 2.0f * (q->y * q->y + q->z * q->z);
    *yaw = atan2f(siny_cosp, cosy_cosp);
}

